home *** CD-ROM | disk | FTP | other *** search
/ io Programmo 60 / IOPROG_60.ISO / soft / c++ / gsl-1.1.1-setup.exe / {app} / src / eigen / qrstep.c < prev    next >
Encoding:
C/C++ Source or Header  |  2001-08-22  |  3.1 KB  |  179 lines
  1. /* remove off-diagonal elements which are neglegible compared with the
  2.    neighboring diagonal elements */
  3.  
  4. inline static void
  5. chop_small_elements (const size_t N, const double d[], double sd[])
  6. {
  7.   double d_i = d[0];
  8.  
  9.   size_t i;
  10.  
  11.   for (i = 0; i < N - 1; i++)
  12.     {
  13.       double sd_i = sd[i];
  14.       double d_ip1 = d[i + 1];
  15.  
  16.       if (fabs (sd_i) < GSL_DBL_EPSILON * (fabs (d_i) + fabs (d_ip1)))
  17.     {
  18.       sd[i] = 0.0;
  19.     }
  20.       d_i = d_ip1;
  21.     }
  22. }
  23.  
  24. /* Generate a Givens rotation (cos,sin) which takes v=(x,y) to (|v|,0) 
  25.  
  26.    From Golub and Van Loan, "Matrix Computations", Section 5.1.8 */
  27.  
  28. inline static void
  29. create_givens (const double a, const double b, double *c, double *s)
  30. {
  31.   if (b == 0)
  32.     {
  33.       *c = 1;
  34.       *s = 0;
  35.     }
  36.   else if (fabs (b) > fabs (a))
  37.     {
  38.       double t = -a / b;
  39.       double s1 = 1.0 / sqrt (1 + t * t);
  40.       *s = s1;
  41.       *c = s1 * t;
  42.     }
  43.   else
  44.     {
  45.       double t = -b / a;
  46.       double c1 = 1.0 / sqrt (1 + t * t);
  47.       *c = c1;
  48.       *s = c1 * t;
  49.     }
  50. }
  51.  
  52. inline static double
  53. trailing_eigenvalue (const size_t n, const double d[], const double sd[])
  54. {
  55.   double ta = d[n - 2];
  56.   double tb = d[n - 1];
  57.   double tab = sd[n - 2];
  58.  
  59.   double dt = (ta - tb) / 2.0;
  60.  
  61.   double mu;
  62.  
  63.   if (dt >= 0)
  64.     {
  65.       mu = tb - (tab * tab) / (dt + hypot (dt, tab));
  66.     }
  67.   else
  68.     {
  69.       mu = tb + (tab * tab) / ((-dt) + hypot (dt, tab));
  70.     }
  71.  
  72.   return mu;
  73. }
  74.  
  75. static void
  76. qrstep (const size_t n, double d[], double sd[], double gc[], double gs[])
  77. {
  78.   double x, z;
  79.   double ak, bk, zk, ap, bp, aq, bq;
  80.   size_t k;
  81.  
  82.   double mu = trailing_eigenvalue (n, d, sd);
  83.  
  84.   x = d[0] - mu;
  85.   z = sd[0];
  86.  
  87.   ak = 0;
  88.   bk = 0;
  89.   zk = 0;
  90.  
  91.   ap = d[0];
  92.   bp = sd[0];
  93.  
  94.   aq = d[1];
  95.  
  96.   if (n == 2)
  97.     {
  98.       double c, s;
  99.       create_givens (x, z, &c, &s);
  100.  
  101.       if (gc != NULL)
  102.         gc[0] = c; 
  103.       if (gs != NULL)
  104.         gs[0] = s;
  105.  
  106.       {
  107.     double ap1 = c * (c * ap - s * bp) + s * (s * aq - c * bp);
  108.     double bp1 = c * (s * ap + c * bp) - s * (s * bp + c * aq);
  109.  
  110.     double aq1 = s * (s * ap + c * bp) + c * (s * bp + c * aq);
  111.  
  112.     ak = ap1;
  113.     bk = bp1;
  114.  
  115.     ap = aq1;
  116.       }
  117.  
  118.       d[0] = ak;
  119.       sd[0] = bk;
  120.       d[1] = ap;
  121.  
  122.       return;
  123.     }
  124.  
  125.   bq = sd[1];
  126.  
  127.   for (k = 0; k < n - 1; k++)
  128.     {
  129.       double c, s;
  130.       create_givens (x, z, &c, &s);
  131.  
  132.       /* store Givens rotation */
  133.       if (gc != NULL)
  134.         gc[k] = c; 
  135.       if (gs != NULL)
  136.         gs[k] = s;
  137.  
  138.       /* compute G' T G */
  139.  
  140.       {
  141.     double bk1 = c * bk - s * zk;
  142.  
  143.     double ap1 = c * (c * ap - s * bp) + s * (s * aq - c * bp);
  144.     double bp1 = c * (s * ap + c * bp) - s * (s * bp + c * aq);
  145.     double zp1 = -s * bq;
  146.  
  147.     double aq1 = s * (s * ap + c * bp) + c * (s * bp + c * aq);
  148.     double bq1 = c * bq;
  149.  
  150.     ak = ap1;
  151.     bk = bp1;
  152.     zk = zp1;
  153.  
  154.     ap = aq1;
  155.     bp = bq1;
  156.  
  157.     if (k < n - 2)
  158.       aq = d[k + 2];
  159.     if (k < n - 3)
  160.       bq = sd[k + 2];
  161.  
  162.     d[k] = ak;
  163.  
  164.     if (k > 0)
  165.       sd[k - 1] = bk1;
  166.  
  167.     if (k < n - 2)
  168.       sd[k + 1] = bp;
  169.  
  170.     x = bk;
  171.     z = zk;
  172.       }
  173.     }
  174.  
  175.   /* k = n - 1 */
  176.   d[k] = ap;
  177.   sd[k - 1] = bk;
  178. }
  179.